Skip to content


ai  101  pytorch  classification  nvidia  cuda  install  tensorrt  yolo  ardupilot  None  ros2  dds  micro ros  xrce  sitl  plugin  SITL  debug  rangefinder  pymavlink  mavros  gazebo  distance sensor  system_time  timesync  cmake  gtest  ctest  cpp  c++  format  fmt  multithreading  spdlog  camera  coordinate system  orb  matching  opencv  build  transformation  computer vision  homography  optical flow  of  trackers  cv  cyclonedds  eprosima  fastdds  simulation  config  ignition  bridge  sdf  tips  ign-transport  sensors  lidar  aptly  apt  encryption  pgp  docker  git  bundle  github  hooks  pre-commit  lxd  container  lxc  x11  profile  vscode  marpit  presentation  marp  markdown  mermaid  video  ffmpeg  gstreamer  cheat-sheet  sdp  v4l2loopback  gi  snippets  cheat Sheet  python  asyncio  future  click  cli  numpy  project  template  black  isort  docs  project document  docstrings  flake8  linter  git-hook  mypy  unittest  pytest  pylint  mock  iterator  generator  logging  tuple  namedtuple  typing  annotation  pyzmq  zmq  msgpack  action  namespace  remap  control2  ros2_control  gdb  qos  tag  plugins  msg  node  zero-copy  shm  tutorial  algorithm  calibration  diff  pid  dev  colcon  colcon_cd  rpi  arm  qemu  settings  behavior  plot  visualization  debugging  diagnostic  diagnostics  tutorials  gst  math  apm  rat_runtime_monitor  web  rosbridge  vue  binding  discovery  gazebo-classic  launch  spawn  cook  gps  imu  ray  gazebo_ros_ray_sensor  ultrsonic  range  ultrasonic  gazebo classic  wrench  effort  odom  ign  gz  xacro  ros_ign  diff_drive  odometry  joint_state  argument  OpaqueFunction  DeclareLaunchArgument  LaunchConfiguration  tmux  nav  slam  test  rclpy  executor  MultiThreadedExecutor  SingleThreadedExecutor  param  dynamic-reconfigure  service  client  setup.py  package.xml  parameter  parameters  custom  msgs  executers  pub  sub  rqt  rviz  rviz2  pose  marker  tf2  deb  package  setup  local_setup  rosdep  package manager  project settings  vcstool  cross-compiler  nano  texture  tmuxp  rootfs  embedded  zah  linux  rm  ubuntu  ip  ss  network  netstat  snap  deploy  ssh  systemd  mkdocs  extensions  socat  networking  serial  udp  tc  mtu  select  px4  robotics  kalman_filter  kalman  filter  control  todo  vscode-ext  json  yaml  schema  yocto  poky  world  gazebo_ros2_control  position_controller  effort_controller  velocity_controller  urdf  gazebo_ros_force  gazebo_ros_joint_state_publisher  robot_state_publisher  joint_state_publisher  projects  vrx  buoyancy 

ROS2 gazebo camera sensor and plugin


Gazebo camera sensor with ROS2 plugin Tutorial Using SDF,

  • Spawn camera SDF model into gazebo
  • View Camera with correct TF in rviz
  • set plugin camera coordinate system (x:right, y:down, z:into the plan)


Image Coordinate Frame#

Camera Coordinate Frame#

Robot Coordinate Frmae#


Demo#

world#

Basic gazebo world - Add simple object viewed by the camera

worlds/camera.world
<?xml version="1.0" ?>
<sdf version="1.5">
  <world name="default">
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <include>
      <uri>model://sun</uri>
    </include>

    <include>
      <pose>2 0 0 0 0 0</pose>
      <uri>model://construction_cone</uri>
    </include>
  </world>
</sdf>

model#

models/camera2/model.sdf
<?xml version="1.0" ?>
<sdf version="1.5">
  <model name="camera2">
    <link name="camera_link">
      <frame name="camera_optical" attached_to="camera_link">
        <pose relative_to="camera_link">0.05 0 0 -1.575 0 -1.575</pose>
      </frame>
      <inertial>
        <mass>0.1</mass>
        <inertia>
          <ixx>0.000166667</ixx>
          <iyy>0.000166667</iyy>
          <izz>0.000166667</izz>
        </inertia>
      </inertial>
      <collision name="collision">
        <geometry>
          <box>
            <size>0.1 0.1 0.1</size>
          </box>
        </geometry>
      </collision>
      <visual name="visual">
        <geometry>
          <box>
            <size>0.1 0.1 0.1</size>
          </box>
        </geometry>
      </visual>
      <sensor name="camera" type="camera">
        <pose>0.05 0 0 0 0 0</pose>
        <camera>
          <horizontal_fov>1.047</horizontal_fov>
          <image>
            <width>320</width>
            <height>240</height>
          </image>
          <clip>
            <near>0.1</near>
            <far>100</far>
          </clip>
        </camera>
        <always_on>1</always_on>
        <update_rate>15</update_rate>
        <visualize>true</visualize>
        <plugin name="camera" filename="libgazebo_ros_camera.so">
          <!-- Change namespace, camera name and topics so -
                 * Images are published to: /custom_ns/custom_camera/custom_image
                 * Camera info is published to: /custom_ns/custom_camera/custom_info
            -->
          <!-- <ros>
              <namespace>custom_ns</namespace>
              <remapping>image_raw:=custom_img</remapping>
              <remapping>camera_info:=custom_info</remapping>
            </ros> -->

          <!-- Set camera name. If empty, defaults to sensor name (i.e. "sensor_name") -->
          <!-- <camera_name>custom_camera</camera_name> -->

          <!-- Set TF frame name. If empty, defaults to link name (i.e. "link_name") -->
          <frame_name>camera_optical</frame_name>
        </plugin>
      </sensor>
    </link> 
  </model>
</sdf>

launch#

launch/camera.launch.py
from launch import LaunchDescription
import os
from math import pi
from ament_index_python.packages import get_package_share_directory
from launch.actions import AppendEnvironmentVariable, DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
import xacro

PACKAGE = "ros2_gazebo_tutorial"
WORLD = "camera.world"
MODEL="camera2"
SDF="model.sdf"

def generate_launch_description():
    ld = LaunchDescription()

    pkg = get_package_share_directory(PACKAGE)
    gazebo_pkg = get_package_share_directory('gazebo_ros')

    verbose = LaunchConfiguration("verbose")
    arg_gazebo_verbose = DeclareLaunchArgument("verbose", default_value="true")
    world = LaunchConfiguration("world")
    arg_gazebo_world = DeclareLaunchArgument("world", default_value=WORLD)
    sim_time = LaunchConfiguration("sim_time")
    arg_sim_time = DeclareLaunchArgument("sim_time", default_value="true")

    resources = [os.path.join(pkg, "worlds")]

    resource_env = AppendEnvironmentVariable(
        name="GAZEBO_RESOURCE_PATH", value=":".join(resources)
    )

    models = [os.path.join(pkg, "models")]

    models_env = AppendEnvironmentVariable(
        name="GAZEBO_MODEL_PATH", value=":".join(models)
    )

    gazebo = IncludeLaunchDescription(
                PythonLaunchDescriptionSource([os.path.join(
                    gazebo_pkg, 'launch', 'gazebo.launch.py')]),
                    launch_arguments={'verbose': verbose, "world": world}.items()
             )

    robot_description_path = os.path.join(pkg, "models", MODEL, SDF)
    doc = xacro.parse(open(robot_description_path))
    xacro.process_doc(doc)

    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[
            {
                'use_sim_time': sim_time, 
                'robot_description': doc.toxml()
            }
        ]
    )

    spawn_entity = Node(
        package="gazebo_ros",
        executable="spawn_entity.py",
        arguments=["-entity", "demo", "-topic", "robot_description", "-z", "0.5"],
        output="screen",
    )

    tf = Node(
            package="tf2_ros",
            executable="static_transform_publisher",
            output="screen",
            arguments=["0","0","0","0","0","0","world","camera_link"]

        )

    tf_camera_optical = Node(
            package="tf2_ros",
            executable="static_transform_publisher",
            output="screen",
            arguments=["0.05","0","0",str(-pi/2),"0",str(-pi/2),"camera_link","camera_optical"]
        )

    rviz_node = Node(
            package='rviz2',
            namespace='',
            executable='rviz2',
            name='rviz2',
            arguments=['-d' + os.path.join(pkg, 'config', 'camera.rviz')]
    )

    ld.add_action(models_env)
    ld.add_action(resource_env)
    ld.add_action(arg_gazebo_verbose)
    ld.add_action(arg_gazebo_world)
    ld.add_action(arg_sim_time)
    ld.add_action(gazebo)
    ld.add_action(robot_state_publisher)
    ld.add_action(spawn_entity)
    ld.add_action(tf)
    ld.add_action(tf_camera_optical)
    ld.add_action(rviz_node)
    return ld

Reference#